#pragma once

#include <fvision/io.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <math.h>
#include <stdio.h>
#include <iostream>
#include "lines.h"
#define LINE_DETAIL 1;
#define FLAG 0;
using namespace std;

//===============================================================================
/*
int TH1,TH2;  // 100 - 50
int sTH1, sTH2;		//for window
int CNTH1a, CNTH1b, CNTH2a, CNTH2b;
int TH_LINEMATCH = 2;			// line matching searches width/TH_L * height/TH_L range for candidates, the larger, the stricter
int sCNTH1a, sCNTH1b, sCNTH2a, sCNTH2b;
int count_entire = 1;		// 2 for displaying entire number of lines   0 for displaying line index     1 for both
char filename1[50], filename2[50], savename1[50], savename2[50],savename3[50], savename4[50];
Line_para para1, para2;
IplImage *src1_c, *src2_c, *src1_d, *src2_d, *src1, *src2, *dst1, *dst2, *color_dst1, *color_dst2;
CvMemStorage *storage1 = cvCreateMemStorage(0), *storage2 = cvCreateMemStorage(0);;
CvSeq *lines1 = 0, *lines2 = 0;
CvFont textfont;
int Muma[255][2];
const int NOP = 6;
CvPoint mouse1[NOP],mouse2[NOP];
int click_count1 = 0, click_count2 = 0;
*/


void ON_LINE1(int a);
void ON_LINE2(int a);
void display(void);
void clear(void);
void Line_corr(void);
int ComputeMatchIndex(double sA, double sB, double *tarA, double *tarB,int Ssize, int i, CvPoint cnt, CvPoint *cnts);
//int ComputeMatchIndex(double sA, double sB, double *tarA, double *tarB,int Ssize, int i);
int ComputeMatchIndex1(int i);
int ComputeMatchIndex2(int i);
void on_mouse1( int event, int x, int y, int flags, void* param);
void on_mouse2( int event, int x, int y, int flags, void* param);
void manual_label(void);
void ComputeVanishingPoint(CvPoint *mouse1, CvPoint *mouse2);
void ComputeVanishingPoint1(void);
void Mutualmatch(void);

void detection_main(int SET){
//	sprintf(filename,"E:/DOC/My Pictures/QuickCam/picture 17.jpg");
	switch(SET){
		case 1:{
			sprintf(filename1,"E:/DATA/Own/Natural-II/Seq/frames/2541.jpg"); // Lib
			sprintf(filename2,"E:/DATA/Own/Natural-II/Seq/frames/2425.jpg");
			break;}
		case 2:{
			sprintf(filename1,"E:/DOC/My Pictures/QuickCam/picture 132.jpg");	//17 23
			sprintf(filename2,"E:/DOC/My Pictures/QuickCam/picture 133.jpg");	//18 24
			break;}
		case 3:{
			sprintf(filename1,"E:/DOC/My Pictures/Drawings/L3.jpg");	// Artificial Example
			sprintf(filename2,"E:/DOC/My Pictures/Drawings/R3.jpg");	//24
			break;}
		case 4:{
			sprintf(filename1,"E:/DOC/Sketches/L2s.jpg");	// Sketch up examples
			sprintf(filename2,"E:/DOC/Sketches/R2s.jpg");	//
			break;}
		default:
			break;
	}
	sprintf(savename1,"E:/DOC/My Pictures/QuickCam/Save/a.jpg");	//24
	sprintf(savename2,"E:/DOC/My Pictures/QuickCam/Save/b.jpg");	//24
	sprintf(savename3,"E:/DOC/My Pictures/QuickCam/Save/tempa.jpg");	//24
	sprintf(savename4,"E:/DOC/My Pictures/QuickCam/Save/tempb.jpg");	//24

//	sprintf(filename,"E:/DATA/ToBeLabeled/0_2_2225.jpg");
//	sprintf(filename,"E:/DATA/Robot/Data_Jeff/Old/im_125.bmp");
//	sprintf(filename,"E:/DATA/ToBeLabeled/0_0_284.jpg");

	src1_c = cvLoadImage(filename1, 1);
	src2_c = cvLoadImage(filename2, 1);
//	cvNamedWindow("testa",1);
//	cvShowImage("testa",src1_c);			//for debugging
	src1_d = cvCloneImage(src1_c);
	src2_d = cvCloneImage(src2_c);
	src1 = cvCreateImage(cvGetSize(src1_c),8,1);
	src2 = cvCreateImage(cvGetSize(src2_c),8,1);
	cvCvtColor(src1_c,src1,CV_RGB2GRAY);
	cvCvtColor(src2_c,src2,CV_RGB2GRAY);
	para1.src = src1;	//src1_c for color one MIGHT boost perf 
	para2.src = src2;	//src2_c for color one
	dst1 = cvCreateImage( cvGetSize(src1), 8, 1 );
    dst2 = cvCreateImage( cvGetSize(src2), 8, 1 );
    color_dst1 = cvCreateImage( cvGetSize(src1), 8, 3 );
    color_dst2 = cvCreateImage( cvGetSize(src2), 8, 3 );
//	cvInitFont(&textfont,CV_FONT_HERSHEY_PLAIN,0.7f,0.6f,.0,1,8);
	cvInitFont(&textfont,CV_FONT_HERSHEY_PLAIN,1.0f,1.0f,.0,1,8);

//	manual_label();
//	ComputeVanishingPoint(mouse1,mouse2);
//	ComputeVanishingPoint1();

//exit_main:

#if LINE_DETAIL

	TH1 = 80;	TH2 = 80;		// 10 works fine
	CNTH1a = 50; CNTH1b = 300;		//0 200
	CNTH2a = 50; CNTH2b = 300;		//0 200

	ON_LINE1(1);
	ON_LINE2(1);
	Line_corr();


	sTH1 = cvCreateTrackbar("Line TH1","color_dst1",&TH1,128,ON_LINE1);
	sCNTH1a = cvCreateTrackbar("Canny TH 1","color_dst1",&CNTH1a,255,ON_LINE1);
	sCNTH1b = cvCreateTrackbar("Canny TH 2","color_dst1",&CNTH1b,512,ON_LINE1);
	sTH2 = cvCreateTrackbar("Line TH2","color_dst2",&TH2,128,ON_LINE2);
	sCNTH2a = cvCreateTrackbar("Canny TH 1","color_dst2",&CNTH2a,255,ON_LINE2);
	sCNTH2b = cvCreateTrackbar("Canny TH 2","color_dst2",&CNTH2b,512,ON_LINE2);
#endif

	cout<<"correspondence:	"<<para1.line_cor[0][0]<<","<<para1.line_cor[0][1]<<" <-> "<< para2.line_cor[0][0] <<","<<para2.line_cor[0][1]<< endl;

	cvWaitKey();
	clear();
}


void Line_corr(){
	//Compute parameters
#if 0
	for ( int i = 0; i<max(para1.lines->total,para2.lines->total); i ++){
		para1.line = (CvPoint*)cvGetSeqElem(para1.lines,i);
		para2.line = (CvPoint*)cvGetSeqElem(para2.lines,i);
		para1.Length[i] = para1.ComputeLength(para1.line);
		para2.Length[i] = para2.ComputeLength(para2.line);
		para1.ComputeLeft(para1.line);
		para2.ComputeLeft(para2.line);
		para1.ComputeRight(para1.line);
		para2.ComputeRight(para2.line);
		para1.ComputeSideA(para1.line);
		para2.ComputeSideA(para2.line);
		para1.ComputeSideB(para1.line);
		para2.ComputeSideB(para2.line);
//		cout << "Length " << i << "	1: " << para1.Length[i] <<"	2: " << para2.Length[i] << "	Left: " << para1.left[i] <<"	right " << para1.right[i] << endl;
		cout << i << " Left: " << para1.left[i] <<"	right " << para1.right[i] << "	sideA " << para1.sideA[i] << "	sideB " << para1.sideB[i] << endl;
	}
#else
	for ( int i = 0; i < para1.lines->total; i ++){
		para1.line = (CvPoint*)cvGetSeqElem(para1.lines,i);
		para1.ComputeLength(para1.line,i);
		para1.ComputeAveI(para1.src);
//		para1.ComputeLeft(para1.line,i);
//		para1.ComputeRight(para1.line,i);
		para1.ComputeSideA(para1.line,i);
		para1.ComputeSideB(para1.line,i);
	}
	for ( int i = 0; i < para2.lines->total; i ++){
		para2.line = (CvPoint*)cvGetSeqElem(para2.lines,i);
		para2.ComputeLength(para2.line,i);
		para2.ComputeAveI(para2.src);
//		para2.ComputeLeft(para2.line,i);
//		para2.ComputeRight(para2.line,i);
		para2.ComputeSideA(para2.line,i);
		para2.ComputeSideB(para2.line,i);
	}
	for ( int i = 0; i < para1.lines->total; i++){
		para1.Corr[i] = ComputeMatchIndex(para1.sideA[i],para1.sideB[i],para2.sideA, para2.sideB, para2.lines->total, i, para1.Center[i], para2.Center);
//		para1.Corr[i] = ComputeMatchIndex(para1.sideA[i],para1.sideB[i],para2.sideA, para2.sideB, para2.lines->total, i);
	}
	for ( int i = 0; i < para2.lines->total; i++){
		para2.Corr[i] = ComputeMatchIndex(para2.sideA[i],para2.sideB[i],para1.sideA, para1.sideB, para1.lines->total, i, para2.Center[i], para1.Center);
//		para2.Corr[i] = ComputeMatchIndex(para2.sideA[i],para2.sideB[i],para1.sideA, para1.sideB, para1.lines->total, i);
	}
	Mutualmatch();

	cout <<	" Average Intensity: " << para1.ave_i << " " << para2.ave_i<< endl;				// Temporarly not considered

	for ( int i = 0; i<max(para1.lines->total,para2.lines->total); i ++){
//		cout << i << " matches: " << para1.Corr[i] << "	"<< para2.Corr[i] << endl;
		cout << i << " Mutual matches: " << Muma[i][1] << "	"<< Muma[i][2] << endl;
	}

#endif 
	//Compare parameters to correspond (assign a corresponder for each of the para1.lines[] elements in para2.lines[]
}

void Mutualmatch(void){				// find the agreement between two groups of matches
	//	para1.Corr & para2.Corr		//get point correspondence 1->2s and 2->1s
	int coup;
	int k = 0;
	for(int i = 0; i < para1.lines->total; i++){			//pick first point in 1, 
//		for(int j =0; j < para2.lines->total; j++){			//for all points in 2s
		coup = para1.Corr[i];		//check if 2 points back to the same point. 2->1
		if((para2.Corr[coup]) == i){			// If so, note this line
			Muma[k][0] = i;					//give new index (according to 1)
			para1.line_cor[k] = para1.line1[i];
			Muma[k][1] = coup;				
			para2.line_cor[k] = para2.line1[i];
			k++;
		}
	}
	//do we need to do the above process from 2 to 1? No. Since mutual is required, both must be true
	// print
}

//int ComputeMatchIndex(double sA, double sB, double *tarA, double *tarB, int Ssize, int i){			//from A to B
//int ComputeMatchIndex(double sA, double sB, double *tarA, double *tarB, int Tsize, int i, CvPoint cnt, CvPoint *cnts){			//from A to B
int ComputeMatchIndex(double sA, double sB, double *tarA, double *tarB, int Tsize, int i, CvPoint cnt, CvPoint *cnts){			//from A to B
	double buf,temp_match = 510;		// max possible value of |A1-A2|+|B1-B2| = [0 510]
	int corr = 0;
//	cvCircle(color_dst1,cnt,2,cvScalar(.0,255.0,255.0,1.0),2,8,0);		// from this line
	for(int j = 0; j <Tsize; j++){		// for every edge in lines
//		if(((cnt.x - cnts[j].x) < src1_c->width/2) && ((cnt.y - cnts[j].y) < src1_c->height/2)){		//if it is adjacent (within a range window of image_size/2)
//		cvCircle(color_dst1,cnts[j],2,cvScalar(.0,.0,255.0,1.0),2,8,0);		// to those lines
		if((abs(cnt.x - cnts[j].x) < src1_c->width/TH_LINEMATCH) && (abs(cnt.y - cnts[j].y) < src1_c->height/TH_LINEMATCH)){		//if it is adjacent (within a range window of image_size/2)
			buf = abs(sA - tarA[j]) + abs(sB - tarB[j]); //compute the difference between these two edges
			if(temp_match > buf){
				temp_match = buf;
//				Line_para::Corr(i) = j;		// return the one with the minimum diff
				corr = j;
			}
		}
//		else	corr = -1;
	}
	return corr;
}
/*
int ComputeMatchIndex1(int i){			//from A to B
	double buf,temp_match = 510;		// max possible value of |A1-A2|+|B1-B2| = [0 510]
	int corr = 0;
	for(int j = 0; j < para1.lines->total; j++){		// for every edge in lines
		buf = abs(para1.sideA[i] - para2.sideA[j]) + abs(para1.sideB[i] - para2.sideB[j]); //compute the difference between these two edges
		if(temp_match > buf){
			temp_match = buf;
//			Line_para::Corr(i) = j;		// return the one with the minimum diff
			corr = j;
		}
	}
	return corr;
}
int ComputeMatchIndex2(int i){			//from A to B
	double buf,temp_match = 510;		// max possible value of |A1-A2|+|B1-B2| = [0 510]
	int corr = 0;
	for(int j = 0; j < para2.lines->total; j++){		// for every edge in lines
		buf = abs(para2.sideA[i] - para1.sideA[j]) + abs(para2.sideB[i] - para1.sideB[j]); //compute the difference between these two edges
		if(temp_match > buf){
			temp_match = buf;
//			Line_para::Corr(i) = j;		// return the one with the minimum diff
			corr = j;
		}
	}
	return corr;
}
*/

int ComputeConstrainedMatchIndex1(int i){			//from A to B
	double buf,temp_match = 510;		// max possible value of |A1-A2|+|B1-B2| = [0 510]
	int corr = 0;
//	CvPoint*
	for(int j = 0; j < para1.lines->total; j++){		// for every edge in lines
		if(para1.lines){		//adjacent){
			buf = abs(para1.sideA[i] - para2.sideA[j]) + abs(para1.sideB[i] - para2.sideB[j]); //compute the difference between these two edges
			if(temp_match > buf){
				temp_match = buf;
	//			Line_para::Corr(i) = j;		// return the one with the minimum diff
				corr = j;
			}
		}
	}
	return corr;
}
int ComputeConstrainedMatchIndex2(int i){			//from A to B
	double buf,temp_match = 510;		// max possible value of |A1-A2|+|B1-B2| = [0 510]
	int corr = 0;
	for(int j = 0; j < para2.lines->total; j++){		// for every edge in lines
		buf = abs(para2.sideA[i] - para1.sideA[j]) + abs(para2.sideB[i] - para1.sideB[j]); //compute the difference between these two edges
		if(temp_match > buf){
			temp_match = buf;
//			Line_para::Corr(i) = j;		// return the one with the minimum diff
			corr = j;
		}
	}
	return corr;
}

void ON_LINE1(int a){
	cvCanny( src1, dst1, CNTH1a, CNTH1b, 3 );
    cvCvtColor( dst1, color_dst1, CV_GRAY2BGR );

#if FLAG
        lines1 = cvHoughLines2( dst1, storage1, CV_HOUGH_STANDARD, 1, CV_PI/180, TH1 + 1, 0, 0 );
	for( int i = 0; i < MIN(lines1->total,100); i++ )
        {
            float* line1 = (float*)cvGetSeqElem(lines1,i);
            float rho1 = line1[0];
            float theta1 = line1[1];
            CvPoint pt1, pt2;
            double a = cos(theta1), b = sin(theta1);
            double x0 = a*rho1, y0 = b*rho1;
            pt1.x = cvRound(x0 + 1000*(-b));
            pt1.y = cvRound(y0 + 1000*(a));
            pt2.x = cvRound(x0 - 1000*(-b));
            pt2.y = cvRound(y0 - 1000*(a));
            cvLine( color_dst1, pt1, pt2, CV_RGB(255,0,0), 1, 8 );
/*
            float* line2 = (float*)cvGetSeqElem(lines2,i);
            float rho2 = line2[0];
            float theta2 = line2[1];
            CvPoint pt3, pt4;
//            double a = cos(theta2), b = sin(theta2);
            a = cos(theta2), b = sin(theta2);
            x0 = a*rho2, y0 = b*rho2;
            pt3.x = cvRound(x0 + 1000*(-b));
            pt3.y = cvRound(y0 + 1000*(a));
            pt4.x = cvRound(x0 - 1000*(-b));
            pt4.y = cvRound(y0 - 1000*(a));
            cvLine( color_dst2, pt3, pt4, CV_RGB(255,0,0), 1, 8 );
*/
		}
#else	
	    lines1 = cvHoughLines2( dst1, storage1, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, TH1+1, 50, 10 );
		char line_index[10], line_count[10];
		para1.lines = lines1;
        for( int i = 0; i < lines1->total; i++ )
        {
//            CvPoint* line1 = (CvPoint*)cvGetSeqElem(lines1,i);
			para1.line1[i] = (CvPoint*)cvGetSeqElem(lines1,i);
			cvLine( color_dst1, para1.line1[i][0], para1.line1[i][1], CV_RGB(255,0,0), 1.5, 8 );
			if((count_entire >= 0) && (count_entire < 2)){
				sprintf(line_index,"%d",i);
				para1.Center[i] = cvPoint((para1.line1[i][0].x + para1.line1[i][1].x)/2,(para1.line1[i][0].y + para1.line1[i][1].y)/2);
//				cvPutText(color_dst1,line_index,cvPoint((line1[0].x + line1[1].x)/2,(line1[0].y + line1[1].y)/2),&textfont,CV_RGB(0,255,0));
				cvPutText(color_dst1,line_index,para1.Center[i],&textfont,CV_RGB(0,255,0));
			}
        }
		if((count_entire >= 1)&&(count_entire <= 2)){
			sprintf(line_count,"%d",lines1->total);
			cvPutText(color_dst1,line_count,cvPoint(color_dst1->width - 25,15),&textfont,CV_RGB(0,255,0));
		}

#endif
	cvNamedWindow("dst1",1);
	cvShowImage("dst1",dst1);
	cvNamedWindow("color_dst1",1);
	cvShowImage("color_dst1",color_dst1);
}

void ON_LINE2(int a){
    cvCanny( src2, dst2, CNTH2a, CNTH2b, 3 );
    cvCvtColor( dst2, color_dst2, CV_GRAY2BGR );
#if FLAG
        lines2 = cvHoughLines2( dst2, storage2, CV_HOUGH_STANDARD, 1, CV_PI/180, TH2 + 1, 0, 0 );

        for( int i = 0; i < MIN(lines1->total,100); i++ )
        {
            float* line1 = (float*)cvGetSeqElem(lines1,i);
            float rho1 = line1[0];
            float theta1 = line1[1];
            CvPoint pt1, pt2;
            double a = cos(theta1), b = sin(theta1);
            double x0 = a*rho1, y0 = b*rho1;
            pt1.x = cvRound(x0 + 1000*(-b));
            pt1.y = cvRound(y0 + 1000*(a));
            pt2.x = cvRound(x0 - 1000*(-b));
            pt2.y = cvRound(y0 - 1000*(a));
            cvLine( color_dst1, pt1, pt2, CV_RGB(255,0,0), 1, 8 );

            float* line2 = (float*)cvGetSeqElem(lines2,i);
            float rho2 = line2[0];
            float theta2 = line2[1];
            CvPoint pt3, pt4;
//            double a = cos(theta2), b = sin(theta2);
            a = cos(theta2), b = sin(theta2);
            x0 = a*rho2, y0 = b*rho2;
            pt3.x = cvRound(x0 + 1000*(-b));
            pt3.y = cvRound(y0 + 1000*(a));
            pt4.x = cvRound(x0 - 1000*(-b));
            pt4.y = cvRound(y0 - 1000*(a));
            cvLine( color_dst2, pt3, pt4, CV_RGB(255,0,0), 1, 8 );

		}
#else
		lines2 = cvHoughLines2( dst2, storage2, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, TH2 + 1, 50, 10 );
		char line_index[10], line_count[10];
		para2.lines = lines2;
		for( int i = 0; i < lines2->total; i++ )
        {
//            CvPoint* line2 = (CvPoint*)cvGetSeqElem(lines2,i);
			para2.line1[i] = (CvPoint*)cvGetSeqElem(lines2,i);
//             cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 1.5, 8 );
            cvLine( color_dst2, para2.line1[i][0], para2.line1[i][1], CV_RGB(255,0,0), 1.5, 8 );
			if((count_entire >= 0) && (count_entire < 2)){
				sprintf(line_index,"%d",i);
				para2.Center[i] = cvPoint((para2.line1[i][0].x + para2.line1[i][1].x)/2,(para2.line1[i][0].y + para2.line1[i][1].y)/2);
//				cvPutText(color_dst2,line_index,cvPoint((line2[0].x + line2[1].x)/2,(line2[0].y + line2[1].y)/2),&textfont,CV_RGB(0,255,0));
				cvPutText(color_dst2,line_index,para2.Center[i],&textfont,CV_RGB(0,255,0));
			}
        }
		if((count_entire >= 1)&&(count_entire <= 2)){
			sprintf(line_count,"%d",lines2->total);
			cvPutText(color_dst2,line_count,cvPoint(color_dst2->width - 25,15),&textfont,CV_RGB(0,255,0));
		}
#endif
	cvNamedWindow("dst2",1);
	cvShowImage("dst2",dst2);
	cvNamedWindow("color_dst2",1);
	cvShowImage("color_dst2",color_dst2);

}

void display(void){
	cvNamedWindow("im1",1);
	cvShowImage("im1",src1_c);
	cvNamedWindow("im2",1);
	cvShowImage("im2",src2_c);
}
void clear(void){	
	cvSaveImage(savename1,src1);
	cvSaveImage(savename2,src2);
	cvDestroyWindow("im1");
	cvDestroyWindow("im2");
	cvDestroyWindow("dst1");
	cvDestroyWindow("dst2");
	cvDestroyWindow("color_dst1");
	cvDestroyWindow("color_dst2");
	cvReleaseImage(&src1);
	cvReleaseImage(&src2);
	cvReleaseImage(&dst1);
	cvReleaseImage(&dst2);
	cvReleaseImage(&color_dst1);
	cvReleaseImage(&color_dst2);
}


void on_mouse1( int event, int x, int y, int flags, void* param){
	if(click_count1 <= NOP)  {
		switch(event){
			case CV_EVENT_LBUTTONUP:
				mouse1[click_count1].x = x;
				mouse1[click_count1].y = y;
				cout << click_count1 << " " << x << "	" << y << endl;
				click_count1++;
				break;
			default:
				break;
		}
	}
 }

void on_mouse2( int event, int x, int y, int flags, void* param){
	if(click_count2 <= NOP)  {
		switch(event){
			case CV_EVENT_LBUTTONUP:
				mouse2[click_count2].x = x;
				mouse2[click_count2].y = y;
				cout << click_count2 << " " << x << "	" << y << endl;
				click_count2++;
				break;
			default:
				break;
		}
	}
 }

void manual_label(void){
	cvNamedWindow("src1",0);
	cvShowImage("src1",src1_d);
	cvNamedWindow("src2",0);
	cvShowImage("src2",src2_d);

	cvSetMouseCallback( "src1", on_mouse1, mouse1 );
	cvSetMouseCallback( "src2", on_mouse2, mouse2 );

//	cout<<"out of mouse callback!"<<endl;

	for(int st = 0; st < 2 ;st ++){		// how to get rid of this "forever"?
		for (int i = 0; i < NOP/2; i++){
			cvLine(src1_d,cvPoint(mouse1[2*i].x,mouse1[2*i].y),cvPoint(mouse1[2*i+1].x,mouse1[2*i+1].y),cvScalar(0.0,255.0,0.0,1.0),1,8,0);
			cvLine(src2_d,cvPoint(mouse2[2*i].x,mouse2[2*i].y),cvPoint(mouse2[2*i+1].x,mouse2[2*i+1].y),cvScalar(0.0,255.0,0.0,1.0),1,8,0);
		}
//		cvWaitKey(0);
		cvShowImage("src1",src1_d);
		cvShowImage("src2",src2_d);
		cvSaveImage(savename3,src1_d);
		cvSaveImage(savename4,src2_d);
		cvWaitKey(0);
	}

}

void ComputeVanishingPoint(CvPoint *mouse1, CvPoint *mouse2){
	double k11, k12, k21, k22;
//	CvPoint *van_can;
	CvPoint van_can[NOP/2];		// vanishing point candidate
	for(int i = 0; i < NOP/2; i++){			// i should always be even number: 0 2 4 6
//		k1 = (mouse1[2*i].y - mouse1[2*i+1].y)/(mouse1[2*i].x - mouse1[2*i+1].x);
		k11 = (mouse1[2*i].x - mouse1[2*i+1].x)?(double)(mouse1[2*i].y - mouse1[2*i+1].y)/(double)(mouse1[2*i].x - mouse1[2*i+1].x):99999.0;
		cout << "k11-"<<i<<"	" << k11 << endl;
//		k21 = (mouse1[2*i].x - mouse1[2*i+1].x)?(mouse1[2*i].y - mouse1[2*i+1].y)/(mouse1[2*i].x - mouse1[2*i+1].x):99999;
//		k22 = (mouse2[2*i].x - mouse2[2*i+1].x)?(mouse2[2*i].y - mouse2[2*i+1].y)/(mouse2[2*i].x - mouse2[2*i+1].x):99999;
		for(int j = 0; j < NOP/2; j++){		// search for every line starting with 2 4 6
			if(i != j){
				k12 = (mouse1[2*j].x - mouse1[2*j+1].x)?(double)(mouse1[2*j].y - mouse1[2*j+1].y)/(double)(mouse1[2*j].x - mouse1[2*j+1].x):99999.0;
				cout << "k12-" <<j<<"	" << k12 << endl;
				van_can[i].x = (mouse1[2*j].y - mouse1[2*i].y + k11*mouse1[2*i].x - k12*mouse1[2*j].x)/(k11 - k12);
				van_can[i].y = mouse1[2*i].y + k11*(mouse1[2*j].y - mouse1[2*i].y - k12*(mouse1[2*j].x - mouse1[2*i].x))/(k11 - k12);
			}
		}
	}
	for(int i = 0; i < NOP/2; i++){
		cout <<" Potential Intercetions: "<< i <<" "<<van_can[i].x <<"," << van_can[i].y << endl;
		cvCircle(src1_d,van_can[i],.02,cvScalar(0.0,0.0,255.0,1.0),2,8,0);		//label vanishing point
	}
	cvShowImage("src1",src1_d);
	cvShowImage("src2",src2_d);
	cvWaitKey(0);
//	para1.Van_point_v = cvPoint(van_can[0]);
}




void ComputeVanishingPoint1(void){
	double k11, k12, k21, k22;
//	CvPoint *van_can;
	CvPoint van_can[NOP/2];		// vanishing point candidate
	for(int i = 0; i < NOP/2; i++){			// i should always be even number: 0 2 4 6
//		k1 = (mouse1[2*i].y - mouse1[2*i+1].y)/(mouse1[2*i].x - mouse1[2*i+1].x);
		k11 = (mouse1[2*i].x - mouse1[2*i+1].x)?(double)(mouse1[2*i].y - mouse1[2*i+1].y)/(double)(mouse1[2*i].x - mouse1[2*i+1].x):99999.0;
		cout << "k11-"<<i<<"	" << k11 << endl;
//		k21 = (mouse1[2*i].x - mouse1[2*i+1].x)?(mouse1[2*i].y - mouse1[2*i+1].y)/(mouse1[2*i].x - mouse1[2*i+1].x):99999;
//		k22 = (mouse2[2*i].x - mouse2[2*i+1].x)?(mouse2[2*i].y - mouse2[2*i+1].y)/(mouse2[2*i].x - mouse2[2*i+1].x):99999;
		for(int j = 0; j < NOP/2; j++){		// search for every line starting with 2 4 6
			if(i != j){
				k12 = (mouse1[2*j].x - mouse1[2*j+1].x)?(double)(mouse1[2*j].y - mouse1[2*j+1].y)/(double)(mouse1[2*j].x - mouse1[2*j+1].x):99999.0;
				cout << "k12-" <<j<<"	" << k12 << endl;
				van_can[i].x = (mouse1[2*j].y - mouse1[2*i].y + k11*mouse1[2*i].x - k12*mouse1[2*j].x)/(k11 - k12);
				van_can[i].y = mouse1[2*i].y + k11*(mouse1[2*j].y - mouse1[2*i].y - k12*(mouse1[2*j].x - mouse1[2*i].x))/(k11 - k12);
			}
		}
	}
	for(int i = 0; i < NOP/2; i++){
		cout <<" Potential Intercetions: "<< i <<" "<<van_can[i].x <<"," << van_can[i].y << endl;
		cvCircle(src1_d,van_can[i],.02,cvScalar(0.0,0.0,255.0,1.0),2,8,0);		//label vanishing point
	}
	cvShowImage("src1",src1_d);
	cvShowImage("src2",src2_d);
	cvWaitKey(0);
//	para1.Van_point_v = cvPoint(van_can[0]);
}

//input: para1.line_cor[0][0] & para1.line_cor[0][1], output para1.line_func[i]
void ComputeLineFunction(void){
	cout << "I'm coverting a group of two points into a group of three values" << endl;
	for(int i = 0; i < para1.lines->total; i++){
		if(para1.line_cor[i] !=NULL){
			double k1 = (double)(para1.line_cor[i][1].y - para1.line_cor[i][0].y)/(double)(para1.line_cor[i][1].x - para1.line_cor[i][0].x);			//k
			para1.line_func[i][0] = k1/(double)(para1.line_cor[i][0].y - k1*para1.line_cor[i][0].x);
			para1.line_func[i][1] = -1.0/(double)(para1.line_cor[i][0].y - k1*para1.line_cor[i][0].x);
			para1.line_func[i][2] = 1.0;
//			cout << para1.line_func[i] << endl;

			double k2 = (double)(para2.line_cor[i][1].y - para2.line_cor[i][0].y)/(double)(para2.line_cor[i][1].x - para2.line_cor[i][0].x);			//k
			para2.line_func[i][0] = k2/(double)(para2.line_cor[i][0].y - k2*para2.line_cor[i][0].x);
			para2.line_func[i][1] = -1.0/(double)(para2.line_cor[i][0].y - k2*para2.line_cor[i][0].x);
			para2.line_func[i][2] = 1.0;
			cout << para1.line_func[i][0] <<" " << para1.line_func[i][1] <<" "<<para1.line_func[i][2] << "	" << para2.line_func[i][0] <<" "<< para2.line_func[i][1] <<" "<< para2.line_func[i][2] << endl;
		}
	}
}

void Normalization(void){

}

void ComputeLineIntersection(void){

}
